#include "serialport.h"
#include <iostream>
using namespace boost::placeholders;

//#define PRINT_MSG

SerialPort::SerialPort(std::string port, int32_t buadrate) : send_buffer_(),
                                                             read_buffer_()
{
    params_.serialPort = port;
    params_.baudRate = buadrate;
    ready = false;
    total_recvbytes = 0;
    ios_ = boost::make_shared<boost::asio::io_service>();
}

void SerialPort::set_timeout(int t){
    timer_->expires_from_now(boost::posix_time::millisec(t));
}

bool SerialPort::is_timeout(){
    return timer_->expires_from_now().is_negative();
}

bool SerialPort::open()
{
    try
    {
        port_ = boost::make_shared<boost::asio::serial_port>(ios_->get_executor().context(), params_.serialPort);
        port_->set_option(boost::asio::serial_port::baud_rate(params_.baudRate));
        port_->set_option(boost::asio::serial_port::flow_control((boost::asio::serial_port::flow_control::type)params_.flowControl));
        port_->set_option(boost::asio::serial_port::parity((boost::asio::serial_port::parity::type)params_.parity));
        port_->set_option(boost::asio::serial_port::stop_bits((boost::asio::serial_port::stop_bits::type)params_.stopBits));
        port_->set_option(boost::asio::serial_port::character_size(8));
        timer_ = boost::make_shared<boost::asio::deadline_timer>(ios_->get_executor().context(), boost::posix_time::seconds(10));
        ready = true;
    }
    catch (std::exception &e)
    {
        ready = false;
        std::cerr << "Failed to open the serial port " << std::endl;
        std::cerr << "Error info: " << e.what() << std::endl;
        return false;
    }

    try
    {
        thread_ = boost::thread(boost::bind(&SerialPort::loop, this));
    }
    catch (std::exception &e)
    {
        std::cerr << "Error Info: " << e.what() << std::endl;
        close();
        return false;
    }

    return true;
}

void SerialPort::run_forever()
{
    ios_->reset();
    boost::asio::io_service::work work(*ios_);
    ios_->run();
}

void SerialPort::loop()
{
    std::cout << "Serial Port Loop Start." << std::endl;
    port_->async_read_some(boost::asio::buffer(_buf),
                        boost::bind(&SerialPort::read_handler, this, _1, _2));

    while (ready)
    {
        // boost::mutex::scoped_lock lock(port_mutex);
        // port_->async_read_some(boost::asio::buffer(_buf),
        //                        boost::bind(&SerialPort::read_handler, this, _1, _2));

        boost::this_thread::sleep_for(boost::chrono::milliseconds(1));
    }
    std::cout << "Serial Port Loop Terminate." << std::endl;
}

void SerialPort::write_handler(const boost::system::error_code &ec)
{
    if (ec)
    {
        std::cerr << "Port Write Error." << std::endl;
        close();
        return;
    }
}

void SerialPort::read_handler(const boost::system::error_code &ec, size_t bytesTransferred)
{
    if (ec)
    {
        if(ready)
            std::cout << "Port Read Error." << std::endl;
        close();
        return;
    }

#if defined(PRINT_MSG)
    std::cout << "I Get: {";
#endif
    boost::mutex::scoped_lock lock(read_mutex);
    for (int i = 0; i < bytesTransferred; i++)
    {
        if (read_buffer_.size() >= SerialBufferSize)
            read_buffer_.pop();

        if(bytesTransferred < _buf.size())
            read_buffer_.push(_buf[i]);
        else
            std::cerr << "[error]: Too much bytes to save" << std::endl;
        
#if defined(PRINT_MSG)
        printf("%02X ", _buf[i]);
#endif
    }
#if defined(PRINT_MSG)
    std::cout << "}" << std::endl;
#endif
    total_recvbytes += bytesTransferred;

    if (read_buffer_.size() >= SerialBufferSize)
        std::cout << "[Warn] buffer is full, old data will be overwitten." << std::endl;

    port_->async_read_some(boost::asio::buffer(_buf),
                               boost::bind(&SerialPort::read_handler, this, _1, _2));

#if defined(PRINT_MSG)
    std::cout << "[INFO] Finish Async Handler" << std::endl;
#endif

}

void SerialPort::close()
{
    boost::mutex::scoped_lock lock(port_mutex);
    try
    {
        if (ready)
        {
            std::cout << "[INFO] Port close." << std::endl;
            port_->close();
            //ios_->stop();
            ready = false;
        }
    }
    catch (std::exception &e)
    {
        std::cout << e.what() << std::endl;
    }    
}

void SerialPort::write(std::vector<uint8_t> &data)
{
    if (ready)
    {
#ifdef DEBUG_MSG
        std::cout << "I Send: ";
        std::for_each(data.begin(), data.end(), [](uint8_t b) { printf("%02X ", b); });
        std::cout << std::endl;
#endif
        boost::mutex::scoped_lock lock(port_mutex);
        try
        {
            port_->write_some(boost::asio::buffer(data));
        }
        catch (std::exception &e)
        {
            std::cout << e.what() << std::endl;
        }
    }
}

uint16_t SerialPort::read()
{
    uint16_t ret = 0xFFFF;
    if (!read_buffer_.empty())
    {
        boost::mutex::scoped_lock lock(read_mutex);
        ret = read_buffer_.front();
        read_buffer_.pop();
    }
    return ret;
}